﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;

namespace Dracham.Classes
{
    public class PathNode : Tile
    {
        public PathNode Parent { get; set; }
        public Boolean HasBeenProcessed { get; set; }
        public Int32 Cost { get; set; }
        public Int32 Heuristic { get; set; }
        public Int32 TotalCost
        {
            get
            {
                return this.Cost + this.Heuristic;
            }
        }

        public PathNode() { }

        public static PathNode CreateFromTile(Tile tile)
        {
            PathNode p = new PathNode();
            p.Position = tile.Position;
            p.IsSolid = tile.IsSolid;

            return p;
        }

        public static PathNode[] GetPathFromParents(PathNode node)
        {
            PathNode[] nodes = new PathNode[node.Cost + 1];
            nodes[node.Cost] = node;

            for (int i = node.Cost; i > 0; i--)
            {
                nodes[i - 1] = nodes[i].Parent;
            }

            return nodes.OrderBy(n => n.Cost).ToArray();
        }
    }

    public class Pathfinder
    {
        private int[,] directions = new int[4, 2]
        {
            {-1,0 }, {0,-1}, {0,1}, { 1, 0}
        }; 

        public List<PathNode> Nodes { get; set; }
        public List<PathNode> OpenNodes
        {
            get
            {
                return this.Nodes.Where(n => !n.HasBeenProcessed && !n.IsSolid).ToList();
            }
        }

        public Pathfinder()
        {
            this.Nodes = new List<PathNode>();
        }

        public static PathNode[] FindPath(Position from, Position to)
        {
            if (from.ZoneId != to.ZoneId)
                return null;

            Pathfinder pathfinder = new Pathfinder();
            foreach (Tile t in from.Zone.Tiles)
            {
                pathfinder.Nodes.Add(PathNode.CreateFromTile(t));
            }

            return pathfinder.GetPathTo(from, to);
        }

        public PathNode[] GetPathTo(Position from, Position to)
        {
            PathNode startNode = this.GetNodeAt(from);

            if (startNode == null)
                return null;

            startNode.Heuristic = this.GetEstimatedDistance(startNode.Position, to);
            startNode.Cost = 0;
            startNode.Parent = null;

            List<PathNode> nodeQueue = new List<PathNode>();
            nodeQueue.Add(startNode);

            while (this.OpenNodes.Count > 0)
            {
                PathNode[] nodesToProcess = nodeQueue.ToArray();
                nodeQueue.Clear();

                foreach (PathNode node in nodesToProcess)
                {
                    if (node.HasBeenProcessed) continue;

                    foreach (PathNode ajdNode in this.GetAdjacentNodes(node))
                    {
                        ajdNode.Heuristic = this.GetEstimatedDistance(ajdNode.Position, to);

                        nodeQueue.Add(ajdNode);
                    }

                    node.HasBeenProcessed = true;

                    if (node.Heuristic == 0)
                    {
                        return PathNode.GetPathFromParents(node);
                    }
                }
            }

            return null;
        }

        public PathNode GetNodeAt(Position pos)
        {
            return this.Nodes.SingleOrDefault(n => n.Position.X == pos.X && n.Position.Y == pos.Y);
        }

        public PathNode[] GetAdjacentNodes(PathNode node)
        {
            List<PathNode> adjacentNodes = new List<PathNode>();

            for (int i = 0; i < 4; i++)
            {
                Int32 x = (int)node.Position.X + directions[i, 0];
                Int32 y = (int)node.Position.Y + directions[i, 1];

                if (x > -1 && y > -1)
                {
                    PathNode adjacentNode = this.GetNodeAt(
                        new Position(x, y, node.Position.ZoneId));

                    if (adjacentNode != null)
                    {
                        if (!adjacentNode.IsSolid && !adjacentNode.HasBeenProcessed)
                        {
                            adjacentNode.Parent = node;
                            adjacentNode.Cost = node.Cost + 1;
                            adjacentNodes.Add(adjacentNode);
                        }
                    }

                }
            }

            return adjacentNodes.ToArray();
        }

        public Int32 GetEstimatedDistance(Position start, Position goal)
        {
            Int32 diagonal = (int)Math.Min(Math.Abs(start.X - goal.X), Math.Abs(start.Y - goal.Y));
            Int32 straight = (int)(Math.Abs(start.X - goal.X) + Math.Abs(start.Y - goal.Y));

            return 2 * diagonal + 1 * (straight - 2 * diagonal);	
        }
    }
}